#ifdef PYBINDINGS
#define NPY_NO_DEPRECATED_API  NPY_1_7_API_VERSION
#include <Python.h>
#include <numpy/arrayobject.h>
#endif


#include "Heli.h"

using namespace sim;

#ifndef PYBINDINGS
int main(int argc, char** argv) {
  const long dt	=  2000;    // usec
  Heli heli;  
  double heli_controls[4];
  heli_controls[2] = 0.2; // coll
  for (int i=0; i<10; i++) {
    heli.step(dt/1000000., heli_controls);
    std::cout << "pos " << heli.cg.NED[0] << " " << heli.cg.NED[1]<< " " << heli.cg.NED[2] << std::endl;
  }
  return 0;
}
#else

#include <cstdio>

static Heli heli;
double heli_controls[4];

static PyObject* dynamic_model_init(PyObject* self, PyObject* args) {
  PyObject *windgust_obj;
  if (!PyArg_ParseTuple(args, "O", &windgust_obj))
    return NULL;
  PyObject *windgust_array = PyArray_FROM_OTF(windgust_obj, NPY_DOUBLE, NPY_ARRAY_WRITEABLE);
  double *windgust    = (double*)PyArray_DATA((PyArrayObject*)windgust_array);
  Py_DECREF(windgust_array);
  printf("init (windgust %f %f %f)\n", windgust[0], windgust[1], windgust[2]);

  heli.wind_params.wind_max[0] = windgust[0];
  heli.wind_params.wind_max[1] = windgust[1];
  heli.wind_params.wind_max[2] = windgust[2];

  return Py_BuildValue("i", 1);
}

static PyObject* dynamic_model_run(PyObject* self, PyObject* args) {
  PyObject *x_obj, *u_obj; double dt;
  if (!PyArg_ParseTuple(args, "OdO", &x_obj, &dt, &u_obj))
    return NULL;
  /* Interpret the input objects as numpy arrays. */
  PyObject *x_array = PyArray_FROM_OTF(x_obj, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
  PyObject *u_array = PyArray_FROM_OTF(u_obj, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
  /* Get pointers to the data as C-types. */
  double *x    = (double*)PyArray_DATA((PyArrayObject*)x_array);
  double *u    = (double*)PyArray_DATA((PyArrayObject*)u_array);

  double ode_dt = 2e-3;
  int nloop = dt/ode_dt;
  //  double rest = dt - nloop*ode_dt;
  //    printf("loop: %d (%f)\n", nloop, rest);
  for (int i=0; i<nloop;i++)
    heli.step(ode_dt, u);
  
  x[0]  = heli.cg.F[0];     x[1]  = heli.cg.F[1];     x[2] =  heli.cg.F[2];
  x[3]  = heli.cg.pqr[0];   x[4]  = heli.cg.pqr[1];   x[5] =  heli.cg.pqr[2];
  x[6]  = heli.cg.NED[0];   x[7]  = heli.cg.NED[1];   x[8] =  heli.cg.NED[2];
  x[9]  = heli.cg.THETA[0]; x[10] = heli.cg.THETA[1]; x[11] = heli.cg.THETA[2];
  x[12] = heli.cg.V[0];     x[13] = heli.cg.V[1];     x[14] = heli.cg.V[2];
  x[15] = heli.m.b1;        x[16] = heli.m.a1;        x[17] = 42.;

  /* Clean up. */
  Py_DECREF(x_array);
  Py_DECREF(u_array);
  return Py_BuildValue("i", 1);
}

/* Bind Python function names to our C functions */
static PyMethodDef dynamic_model_methods[] = {
  {"init", dynamic_model_init, METH_VARARGS},
  {"run",  dynamic_model_run,  METH_VARARGS},
  {NULL, NULL}
};

/* Python calls this to let us initialize our module */
PyMODINIT_FUNC initlibpat_dm_hk(void) {
  (void) Py_InitModule("libpat_dm_hk", dynamic_model_methods);
  /* Load `numpy` functionality. */
  import_array();
}

#endif /* PYBINDINGS */
